/*
Copyright 2010 GHI Electronics LLC
Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. 
*/

using System;
using System.Threading;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.Hardware;
namespace GHIElectronics.NETMF.FEZ
{
    static class FEZMini_Robot
    {
        static OutputCompare oc = new OutputCompare((Cpu.Pin)FEZ_Pin.Digital.An0, true, 5);
        static uint[] timings = new uint[2];

        static PWM _pwm1 = new PWM((PWM.Pin)FEZ_Pin.PWM.Di5);
        static OutputPort _dir1 = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di3, false);

        static PWM _pwm2 = new PWM((PWM.Pin)FEZ_Pin.PWM.Di6);
        static OutputPort _dir2 = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di9, false);

        static sbyte _last_speed_left = 0, _last_speed_right = 0;


        static public int PIEZO_RESONATE_FREQUENCY
        {
            get
            {
                return 8200;
            }
        }

        static public void Beep(int freq, int duration_mSec)
        {
            if (freq > 10000)
                throw new Exception("Frequency is too high");

            timings[0] = timings[1] = (uint)((long)1000000 / (freq / 2));
            oc.Set(true, timings, 0, 2, true);
            Thread.Sleep(duration_mSec);
            oc.Set(false);

        }
		
        static public void Move(sbyte speed_left, sbyte speed_right)
        {
            if (speed_left > 100 || speed_left < -100 || speed_right > 100 || speed_right < -100)
                new ArgumentException();

            _last_speed_left = speed_left;
            _last_speed_right = speed_right;


            if (speed_left < 0)
            {
                _dir1.Write(true);
                _pwm1.Set(1000, (byte)(100 - Math.Abs(speed_left)));
            }
            else
            {
                _dir1.Write(false);
                _pwm1.Set(1000, (byte)speed_left);
            }

            ////////////////////////////

            if (speed_right < 0)
            {
                _dir2.Write(true);
                _pwm2.Set(1000, (byte)(100 - Math.Abs(speed_right)));

            }
            else
            {
                _dir2.Write(false);
                _pwm2.Set(1000, (byte)speed_right);
            }
        }
		
        static public void MoveRamp(sbyte speed_left, sbyte speed_right, byte ramping_delay)
        {
            sbyte temp_speed_left = _last_speed_left;
            sbyte temp_speed_right = _last_speed_right;

            while ((speed_left != temp_speed_left) ||
                    (speed_right != temp_speed_right))
            {
                if (temp_speed_left > speed_left)
                    temp_speed_left--;
                if (temp_speed_left < speed_left)
                    temp_speed_left++;

                if (temp_speed_right > speed_right)
                    temp_speed_right--;
                if (temp_speed_right < speed_right)
                    temp_speed_right++;

                Move(temp_speed_left, temp_speed_right);
                Thread.Sleep(ramping_delay);
            }
        }
    }
}